#include "socketlib.h"

using namespace std;



void send_data(float angel[6],float pose[6])
{
    const unsigned short SERVERPORT = 6000;

    const char* SERVER_IP = "127.0.0.1";
    //const char* SERVER_IP = "192.168.1.100";
    int sock, client_fd;

    sock=socket_connect(SERVER_IP,SERVERPORT);
    //cout<<sock<<endl;
    while(1)
    {
        cout<<"send:"<<endl;
        struct robot_info send_pack; //定义结构体变量
        char snd_buf[1024];


        send_pack.cmd1=1;
        send_pack.cmd2=0;
        send_pack.cmd3=0;
        send_pack.cmd4=0;

        send_pack.l_angel1=angel[0];
        send_pack.l_angel2=angel[1];
        send_pack.l_angel3=angel[2];
        send_pack.l_angel4=angel[3];
        send_pack.l_angel5=angel[4];
        send_pack.l_angel6=angel[5];

        send_pack.r_angel1=angel[0];
        send_pack.r_angel2=angel[1];
        send_pack.r_angel3=angel[2];
        send_pack.r_angel4=angel[3];
        send_pack.r_angel5=angel[4];
        send_pack.r_angel6=angel[5];

        send_pack.l_x=pose[0];
        send_pack.l_y=pose[1];
        send_pack.l_z=pose[2];
        send_pack.l_a=pose[3];
        send_pack.l_b=pose[4];
        send_pack.l_c=pose[5];

        send_pack.r_x=pose[0];
        send_pack.r_y=pose[1];
        send_pack.r_z=pose[2];
        send_pack.r_a=pose[3];
        send_pack.r_b=pose[4];
        send_pack.r_c=pose[5];

        send_pack.left_wheel=0;
        send_pack.right_wheel=0;
        send_pack.up_down=0;

        memset(snd_buf,0,112);
        memcpy(snd_buf,&send_pack,sizeof(send_pack)); //结构体转换成字符串

        int needSend=sizeof(send_pack);

        send(sock,snd_buf,needSend,0);//发送信息


        sleep(1);

        //close(sock);
        //break;

    }

    //close(sock);
}


int main()
{
    // send message
    float pose[6];
    float angel[6];
    memset(pose,0, sizeof(pose));
    memset(angel,0, sizeof(angel));

    angel[0]=0;
    angel[1]=0;
    angel[2]=0;
    angel[3]=0;
    angel[4]=0;
    angel[5]=1;

    pose[0]=2;
    pose[1]=0;
    pose[2]=0;
    pose[3]=0;
    pose[4]=0;
    pose[5]=0;


    send_data(angel,pose);
}//
// Created by immo on 17-12-21.
//

